//% color="#ff9f06" iconWidth=50 iconHeight=40
namespace customNamespace {

    //% block="初始化差动驱动机器人 轮子间距[L]" blockType="command"
    //% L.shadow="number" L.defl=500
    export function initializeDifferentialDriveRobot(parameter: any, block: any) {
        var L = parameter.L.code;
        Generator.addCode(`
class DifferentialDriveRobot:
    def __init__(self, L):
        self.L = L  # 轮子间距

    def calculate_velocity_and_angular_velocity(self, v_r, v_l):
        v = (v_r + v_l) / 2
        omega = (v_r - v_l) / self.L
        return v, omega

    def calculate_wheel_speeds(self, v, omega):
        v_r = v + (omega * self.L) / 2
        v_l = v - (omega * self.L) / 2
        return v_r, v_l

robot = DifferentialDriveRobot(L=${L})
        `);
    }

    //% block="计算车速和角速度 右轮速度[VR] 左轮速度[VL]" blockType="command"
    //% VR.shadow="number" VR.defl=0
    //% VL.shadow="number" VL.defl=0
    export function calculateVelocityAndAngularVelocity(parameter: any, block: any) {
        var vr = parameter.VR.code;
        var vl = parameter.VL.code;
        Generator.addCode(`
v_r = ${vr}
v_l = ${vl}
v, omega = robot.calculate_velocity_and_angular_velocity(v_r, v_l)
print(f"车速: v = {v:.2f} 毫米/秒, 角速度: omega = {omega:.2f} 弧度/秒")
        `);
    }

    //% block="计算左右轮速度 线速度[V] 角速度[OMEGA]" blockType="command"
    //% V.shadow="number" V.defl=0
    //% OMEGA.shadow="number" OMEGA.defl=0
    export function calculateWheelSpeeds(parameter: any, block: any) {
        var v = parameter.V.code;
        var omega = parameter.OMEGA.code;
        Generator.addCode(`
v = ${v}
omega = ${omega}
v_r, v_l = robot.calculate_wheel_speeds(v, omega)
print(f"右轮速度: v_r = {v_r:.2f} 毫米/秒, 左轮速度: v_l = {v_l:.2f} 毫米/秒")
        `);
    }

    //% block="启动 WebSocket 服务器" blockType="command"
    export function startWebSocketServer(parameter: any, block: any) {
        Generator.addImport('import asyncio');
        Generator.addImport('import websockets');
        Generator.addImport('import json');
        Generator.addCode(`
data_list_velocity = []
data_list_wheel = []

async def send_data(websocket, path):
    global data_list_velocity, data_list_wheel
    while True:
        if data_list_velocity or data_list_wheel:
            message = json.dumps({
                "velocity_data": data_list_velocity,
                "wheel_data": data_list_wheel
            })
            await websocket.send(message)
            print(f"Sent: {message}")
            try:
                response = await websocket.recv()
                print(f"Received: {response}")
            except websockets.exceptions.ConnectionClosedOK:
                print("Connection closed by client")
            await asyncio.sleep(5)  # 每5秒发送一次数据
        else:
            await asyncio.sleep(1)  # 如果没有数据，等待1秒后重试

async def main():
    async with websockets.serve(send_data, "0.0.0.0", 8765):
        await asyncio.Future()  # run forever

def update_velocity_and_angular_velocity(count, velocity, angular_velocity):
    global data_list_velocity
    for i in range(count):
        data_list_velocity.append({
            "time": count,  # 使用count的值作为时间
            "velocity": velocity,
            "angular_velocity": angular_velocity
        })
    print(f"Updated data_list_velocity: {data_list_velocity}")

def update_wheel_speeds(count, left_wheel, right_wheel):
    global data_list_wheel
    for i in range(count):
        data_list_wheel.append({
            "time": count,  # 使用count的值作为时间
            "left_wheel": left_wheel,
            "right_wheel": right_wheel
        })
    print(f"Updated data_list_wheel: {data_list_wheel}")

# 创建一个事件循环
loop = asyncio.get_event_loop()
        `);
    }

    //% block="添加 WebSocket 车速数据 发送次数:[COUNT] 次 车速:[VELOCITY] 和角速度:[ANGULAR_VELOCITY]" blockType="command"
    //% COUNT.shadow="number" COUNT.defl=1
    //% VELOCITY.shadow="number" VELOCITY.defl=1.0
    //% ANGULAR_VELOCITY.shadow="number" ANGULAR_VELOCITY.defl=0.5
    export function addVelocityData(parameter: any, block: any) {
        let count = parameter.COUNT.code;
        let velocity = parameter.VELOCITY.code;
        let angular_velocity = parameter.ANGULAR_VELOCITY.code;
        Generator.addCode(`
update_velocity_and_angular_velocity(${count}, ${velocity}, ${angular_velocity})
        `);
    }

    //% block="添加 WebSocket 轮速数据 发送次数:[COUNT] 次 左轮速度:[LEFT_WHEEL] 和右轮速度:[RIGHT_WHEEL]" blockType="command"
    //% COUNT.shadow="number" COUNT.defl=1
    //% LEFT_WHEEL.shadow="number" LEFT_WHEEL.defl=1.0
    //% RIGHT_WHEEL.shadow="number" RIGHT_WHEEL.defl=1.0
    export function addWheelData(parameter: any, block: any) {
        let count = parameter.COUNT.code;
        let left_wheel = parameter.LEFT_WHEEL.code;
        let right_wheel = parameter.RIGHT_WHEEL.code;
        Generator.addCode(`
update_wheel_speeds(${count}, ${left_wheel}, ${right_wheel})
        `);
    }

    //% block="运行 WebSocket 服务器" blockType="command"
    export function runWebSocketServer(parameter: any, block: any) {
        Generator.addCode(`
# 运行 WebSocket 服务器
loop.run_until_complete(main())
        `);
    }
}
